#ifndef LIBBIOMIMETICS_ORIENTATION_TOOLS_H
#define LIBBIOMIMETICS_ORIENTATION_TOOLS_H

#include "Eigen/Core"
template <typename T>
using Vec3 = typename Eigen::Matrix<T, 3, 1>;
/*!
 * Convert a quaternion to RPY.  Uses ZYX order (yaw-pitch-roll), but returns
 * angles in (roll, pitch, yaw).
 */

template <typename T>
Vec3<typename T::Scalar> quatToRPY(const T& q) {
    static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 4,
                  "Must have 4x1 quat");
    Vec3<typename T::Scalar> rpy;
    typename T::Scalar as = std::min(-2. * (q[1] * q[3] - q[0] * q[2]), .99999);
    rpy(2) =
            std::atan2(2 * (q[1] * q[2] + q[0] * q[3]),
                       (q[0])*(q[0]) + (q[1])*(q[1]) - (q[2])*(q[2]) - (q[3])*(q[3]));
    rpy(1) = std::asin(as);
    rpy(0) =
            std::atan2(2 * (q[2] * q[3] + q[0] * q[1]),
                       (q[0])*(q[0]) - (q[1])*(q[1]) - (q[2])*(q[2]) + (q[3])*(q[3]));
    return rpy;
}

#endif